#include<cv_bridge/cv_bridge.h>
#include<opencv2/opencv.hpp>
#include <sensor_msgs/Image.h>
#include <ros/ros.h>
#include<nav_msgs/Odometry.h>

//apriltag
#include<apriltag/tag36h11.h>
#include<apriltag/apriltag.h>
#include<apriltag/common/homography.h>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Eigen>

#include"/home/nrsl/keti/huangsx/ros_learning/src/apriltag_detction/include/apriltag_detction/gimbal_controller.h"

using namespace std;

cv::Mat img_,img_gray_;
cv::Mat	map1_,map2_,camera_matrix_new_;
double camera_matrix_array_[9] = {612.784362792969,0.0,323.529235839844,
                                0.0,613.156555175781,242.369125366211,
                                0.0,0.0,1.0};
double dist_coeffs_array_[4] = {-0.0463542121479508,
            0.037809855226605,
            -0.00213526365876957,
            0.00181764404964257};
cv::Mat	camera_matrix_(3,3,CV_64F,(double*)camera_matrix_array_);
cv::Mat dist_coeffs_(4,1,CV_64F,(double*)dist_coeffs_array_);

double tag_size_ = 0.2365;

apriltag_family_t *tf_ = NULL;
apriltag_detector_t *td_ = NULL;

ros::Publisher tag0_odom_pub_;

//gimbal control
Eigen::Matrix4d T_camera_livox_;

RealGimbalController real_gimbal_controller_("/dev/ttyUSB1");

double target_pitch_last_=100;
double target_yaw_last_=100;


void addObjectPoints (
    double s, cv::Matx44d T_oi, std::vector<cv::Point3d >& objectPoints)
{
  // Add to object point vector the tag corner coordinates in the bundle frame
  // Going counterclockwise starting from the bottom left corner
  objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s,-s, 0, 1));
  objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d( s,-s, 0, 1));
  objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d( s, s, 0, 1));
  objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s, s, 0, 1));
}

Eigen::Matrix4d getRelativeTransform(
    std::vector<cv::Point3d > objectPoints,
    std::vector<cv::Point2d > imagePoints,
    double fx, double fy, double cx, double cy)
{
  // perform Perspective-n-Point camera pose estimation using the
  // above 3D-2D point correspondences
  cv::Mat rvec, tvec;
  cv::Matx33d cameraMatrix(fx,  0, cx,
                           0,  fy, cy,
                           0,   0,  1);
  cv::Vec4f distCoeffs(0,0,0,0); // distortion coefficients
  // TODO Perhaps something like SOLVEPNP_EPNP would be faster? Would
  // need to first check WHAT is a bottleneck in this code, and only
  // do this if PnP solution is the bottleneck.
  cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
  cv::Matx33d R;
  cv::Rodrigues(rvec, R);
  Eigen::Matrix3d wRo;
  wRo << R(0,0), R(0,1), R(0,2), R(1,0), R(1,1), R(1,2), R(2,0), R(2,1), R(2,2);

  Eigen::Matrix4d T; // homogeneous transformation matrix
  T.topLeftCorner(3, 3) = wRo;
  T.col(3).head(3) <<
      tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
  T.row(3) << 0,0,0,1;
  return T;
}

void addImagePoints (
    apriltag_detection_t *detection,
    std::vector<cv::Point2d >& imagePoints)
{
  // Add to image point vector the tag corners in the image frame
  // Going counterclockwise starting from the bottom left corner
  double tag_x[4] = {-1,1,1,-1};
  double tag_y[4] = {1,1,-1,-1}; // Negated because AprilTag tag local
                                 // frame has y-axis pointing DOWN
                                 // while we use the tag local frame
                                 // with y-axis pointing UP
  for (int i=0; i<4; i++)
  {
    // Homography projection taking tag local frame coordinates to image pixels
    double im_x, im_y;
    homography_project(detection->H, tag_x[i], tag_y[i], &im_x, &im_y);
    imagePoints.push_back(cv::Point2d(im_x, im_y));
  }
}

void img_CB(const sensor_msgs::ImageConstPtr& image_msg){
    cv_bridge::CvImagePtr image_front;
    image_front = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
    img_ = (*image_front).image;
    cv::cvtColor(img_, img_gray_, CV_BGR2GRAY);

    //全图像畸变 可以通过参数控制是否保留原图像的所有像素
    // camera_matrix_new_ = cv::getOptimalNewCameraMatrix(camera_matrix_, dist_coeffs_, img_.size(), 1, img_.size(), 0);
    // cv::initUndistortRectifyMap(camera_matrix_, dist_coeffs_, cv::Mat(),
    //     camera_matrix_new_,
    //     img_.size(), CV_32FC1, map1_, map2_);
    // cout<<"new camera m "<<cv::getOptimalNewCameraMatrix(camera_matrix_, dist_coeffs_, img_.size(), 1, img_.size(), 0)<<endl;
    // cout<<"raw camera m"<<camera_matrix_<<endl;
    //这两个相机内参还是不一样的，需要将新的相机内参保留，用于后面的apriltag位姿计算
    
    // 
	// cv::fisheye::initUndistortRectifyMap(camera_matrix_, dist_coeffs_, cv::Mat(),
    //     camera_matrix_,
	// 	img_.size(), CV_32FC1, map1_, map2_);
    // cv::remap(img_, img_undistort_, map1_, map2_, cv::INTER_CUBIC);

    // Make an image_u8_t header for the Mat data
    image_u8_t im = { .width = img_gray_.cols,
        .height = img_gray_.rows,
        .stride = img_gray_.cols,
        .buf = img_gray_.data
    };

    zarray_t *detections = apriltag_detector_detect(td_, &im);

    // Draw detection outlines
    for (int i = 0; i < zarray_size(detections); i++) {
        apriltag_detection_t *det;
        zarray_get(detections, i, &det);
        cv::line(img_, cv::Point(det->p[0][0], det->p[0][1]),
                    cv::Point(det->p[1][0], det->p[1][1]),
                    cv::Scalar(0, 0, 255), 2);
        cv::line(img_, cv::Point(det->p[0][0], det->p[0][1]),
                    cv::Point(det->p[3][0], det->p[3][1]),
                    cv::Scalar(0, 255, 0), 2);
        cv::line(img_, cv::Point(det->p[1][0], det->p[1][1]),
                    cv::Point(det->p[2][0], det->p[2][1]),
                    cv::Scalar(0xff, 0, 0), 2);
        cv::line(img_, cv::Point(det->p[2][0], det->p[2][1]),
                    cv::Point(det->p[3][0], det->p[3][1]),
                    cv::Scalar(0xff, 0, 0), 2);

        stringstream ss;
        ss << det->id;
        cv::String text = ss.str();
        int fontface = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
        double fontscale = 1.0;
        int baseline;
        cv::Size textsize = getTextSize(text, fontface, fontscale, 2,
                                        &baseline);
        putText(img_, text, cv::Point(det->c[0]-textsize.width/2,
                                    det->c[1]+textsize.height/2),
                fontface, fontscale, cv::Scalar(0xff, 0x99, 0), 2);
        
        std::vector<cv::Point3d > standaloneTagObjectPoints;
        std::vector<cv::Point2d > standaloneTagImagePoints;
        addObjectPoints(tag_size_/2, cv::Matx44d::eye(), standaloneTagObjectPoints);
        addImagePoints(det, standaloneTagImagePoints);
        Eigen::Matrix4d transform = getRelativeTransform(standaloneTagObjectPoints,
                                                        standaloneTagImagePoints,
                                                        camera_matrix_.at<double>(0,0), camera_matrix_.at<double>(1,1), camera_matrix_.at<double>(0,2), camera_matrix_.at<double>(1,2));

        if(det->id==0){
            transform = T_camera_livox_.inverse()*transform;
            Eigen::Matrix3d rot = transform.block(0, 0, 3, 3);
            Eigen::Quaterniond rot_quaternion(rot);
            Eigen::Quaterniond q_temp(Eigen::AngleAxisd(-M_PI/2,Eigen::Vector3d::UnitX()));
            rot_quaternion = rot_quaternion*q_temp;
            q_temp = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI/2,Eigen::Vector3d::UnitZ()));
            rot_quaternion = rot_quaternion*q_temp;
            transform.block(0,0,3,3) = rot_quaternion.toRotationMatrix();

            Eigen::Matrix4d T_plan_livox = Eigen::Matrix4d::Identity();
            Eigen::Quaterniond q_plan(Eigen::AngleAxisd(real_gimbal_controller_.getYaw(),Eigen::Vector3d::UnitZ()));
            q_temp = Eigen::Quaterniond(Eigen::AngleAxisd(-real_gimbal_controller_.getPitch(),Eigen::Vector3d::UnitY()));
            q_plan = q_plan*q_temp;
            T_plan_livox.block(0,0,3,3) = q_plan.toRotationMatrix();
            transform = T_plan_livox*transform;
            double target_yaw = atan2(transform(1,3),transform(0,3));
            double target_pitch = atan2(transform(2,3)-0.5,transform(0,3)-0.1);
            std::cout<<"r pitch "<<real_gimbal_controller_.getPitch()<<"r yaw "<<real_gimbal_controller_.getYaw()<<endl;
            cout<<"pitch "<<target_pitch<<"yaw "<<target_yaw<<endl;
            if(abs(target_pitch-target_pitch_last_)<0.01)
                target_pitch = target_pitch_last_;
            else
                target_pitch_last_ = target_pitch;
            if(abs(target_yaw-target_yaw_last_)<0.01)
                target_yaw = target_yaw_last_;
            else
                target_yaw_last_ = target_yaw;

            
            Eigen::Matrix3d rot_show = transform.block(0, 0, 3, 3);
            Eigen::Quaterniond rot_q_show(rot_show);

            //pub odom
            nav_msgs::Odometry tag_odom_cur_msg;
            tag_odom_cur_msg.header.frame_id = "camera";
            tag_odom_cur_msg.child_frame_id = "tag0";
            tag_odom_cur_msg.header.stamp = ros::Time::now();
            geometry_msgs::Pose pose_msg_temp;
            pose_msg_temp.position.x = transform(0,3);
            pose_msg_temp.position.y = transform(1,3);
            pose_msg_temp.position.z = transform(2,3);
            pose_msg_temp.orientation.w = rot_q_show.w();
            pose_msg_temp.orientation.x = rot_q_show.x();
            pose_msg_temp.orientation.y = rot_q_show.y();
            pose_msg_temp.orientation.z = rot_q_show.z();
            tag_odom_cur_msg.pose.pose = pose_msg_temp;
            tag_odom_cur_msg.twist.twist.linear.x = -real_gimbal_controller_.getPitch();
            tag_odom_cur_msg.twist.twist.linear.y = real_gimbal_controller_.getYaw();
            tag_odom_cur_msg.twist.twist.linear.z = 0;
            tag0_odom_pub_.publish(tag_odom_cur_msg);
                
            real_gimbal_controller_.sendPitch(target_pitch);
            // ros::Duration(0.05).sleep();
            double yaw_last = real_gimbal_controller_.getYaw();
            double interval_yaw = (target_yaw-yaw_last)/10;
            for(int i=0;i<10;i++){
                real_gimbal_controller_.sendYaw(yaw_last+interval_yaw*i);
                ros::Duration(0.05).sleep();
            }
            real_gimbal_controller_.sendYaw(target_yaw);
            // real_gimbal_controller_.sendPitch(1);
            // real_gimbal_controller_.sendYaw(1);
            // ros::Duration(0.05).sleep();
        }


        // geometry_msgs::PoseWithCovarianceStamped tag_pose =
        //     makeTagPose(transform, rot_quaternion, image->header);
    }
    apriltag_detections_destroy(detections);

    imshow("Tag Detections", img_);
    cv::waitKey(1);
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "det_img");
    ros::NodeHandle nh;

    tf_ = tag36h11_create();
    td_ = apriltag_detector_create();
    apriltag_detector_add_family(td_, tf_);
    td_->quad_decimate = 1.0;
    td_->quad_sigma = 0.0;
    td_->nthreads = 2;
    td_->debug = 0;
    td_->refine_edges = 1;

    T_camera_livox_<<0.000509857,-0.999954,-0.00956595,0.0128984,
                                        -0.0100791,0.00956033,-0.999904,-0.0734674,
                                        0.999949,0.000606224,-0.0100737,-0.0226611,
                                        0,0,0,1;
    
    ros::Subscriber image_sub=nh.subscribe("/camera/color/image_raw", 5,img_CB);
    tag0_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/tag/odom", 1);


    char a = '0';
    cout<<"debug  0 "<<int(a)<<endl;
    a = '1';
    cout<<"debug  1 "<<int(a)<<endl;
    a = '2';
    cout<<"debug  2 "<<int(a)<<endl;
    a = '3';
    cout<<"debug  3 "<<int(a)<<endl;
    a = '4';
    cout<<"debug  4 "<<int(a)<<endl;
    a = '5';
    cout<<"debug  5 "<<int(a)<<endl;
    a = '6';
    cout<<"debug  6 "<<int(a)<<endl;
    a = '7';
    cout<<"debug  7 "<<int(a)<<endl;
    a = '8';
    cout<<"debug  8 "<<int(a)<<endl;
    a = '9';
    cout<<"debug  9 "<<int(a)<<endl;
    real_gimbal_controller_.sendPitch(0);
    real_gimbal_controller_.sendYaw(0);

    // while(1){
    //     real_gimbal_controller_.sendPitch(0);
    //     ros::Duration(2).sleep();
    //     // real_gimbal_controller_
    // }


    ros::spin();

    return 0;
}
